/**
 * @file PipeLineProcess.cpp
 * @author xag (xag@xa.com)
 * @brief
 * @version 1.0.10
 * @date 2023-04-21
 *
 * @copyright Copyright (c) 2007-2023, Guangzhou Xaircraft Technology Co., Ltd.
 *
 */

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <fcntl.h>
#include <stdint.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <signal.h>
#include <poll.h>
#include <unistd.h>
#include <dirent.h>

#include "EncoderProcess.h"
#include "XagLog.h"
#include "XagTypes.h"

#ifdef DRMMODE
#include <xf86drmMode.h>
#else
#ifdef DRM
#include <drm.h>
#endif /* DRM */
#endif /* DRMMODE */

// #define DEBUG
#ifdef DEBUG
#define debug_printf(fmt, arg...) XAG_LOG_I("enc: " fmt, ##arg);
#else
#define debug_printf(fmt, arg...)
#endif

using std::thread;


static const struct tienc_format out_formats[] = {
    {
        .name = const_cast<char*>("NV12"),
        .type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
        .fourcc = V4L2_PIX_FMT_NV12,
        .size_num = 3,
        .size_den = 2,
        .n_planes = 1,
        .bytes_pp = 1,
    },
    {
        .name = const_cast<char*>("RGBA"),
        .type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
        .fourcc = V4L2_PIX_FMT_RGB32,
        .size_num = 1,
        .size_den = 1,
        .n_planes = 1,
        .bytes_pp = 4,
    }
};

static const struct tienc_format cap_formats[] = {
    {
        .name = const_cast<char*>("H264"),
        .type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
        .fourcc = V4L2_PIX_FMT_H264,
        .size_num = 2,
        .size_den = 1,
        .n_planes = 1,
        .bytes_pp = 1,
    }
};


namespace kapok_hardware_tiencoder {

EncoderProcess::EncoderProcess() {
}

EncoderProcess::~EncoderProcess() {
}

int EncoderProcess::QueueBuffer(int fd, int index,
            struct tienc_buffer buffer,
            struct tienc_format format) {
    struct v4l2_buffer buf;
    struct v4l2_plane buf_planes[MAX_PLANES];
    int ret = 0;

    memset(&buf, 0, sizeof(buf));
    memset(&buf_planes, 0, sizeof(buf_planes));

    if (V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE == format.type)
        buf_planes[0].bytesused = format.size;

    if (format.memory == V4L2_MEMORY_MMAP) {
        buf_planes[0].m.mem_offset = buffer.offset;
    } else if (format.memory == V4L2_MEMORY_DMABUF) {
        buf_planes[0].m.fd = buffer.dbuf_fd;
        buf_planes[0].length = buffer.length;
    } else {
        debug_printf("Invalid memory type selected\n");
        return -1;
    }

    buf.index = index;
    buf.type = format.type;
    buf.memory = format.memory;
    buf.m.planes = buf_planes;
    buf.length = format.n_planes;

    ret = ioctl(fd, VIDIOC_QBUF, &buf);
    if (ret < 0) {
        debug_printf("%s QBUF buffer %d failed %d: %s\n",
               (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == format.type) ?
               ("CAPTURE") : ("OUTPUT"), index, errno, strerror(errno));
    } else {
        debug_printf("%s QBUF buffer %d success\n",
                 (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == format.type)
                 ? ("CAPTURE") : ("OUTPUT"), index);
    }

    return ret;
}

int EncoderProcess::HandleOutbuf(int fd, int rdfd, int index,
             struct tienc_buffer buffer,
             struct tienc_format format,
             int n_frames,
             char *yuv_data) {
    int ret = 0;
    static int frame_count = 1; /* Start at 1 to send EOS before last frame */
    struct v4l2_encoder_cmd cmd = {0};
    static unsigned long ac = 1;

    debug_printf("%s Enter\n", __func__);

    if (frame_count == n_frames) {
        cmd.cmd = V4L2_ENC_CMD_STOP;
        cmd.flags = V4L2_ENC_CMD_STOP_AT_GOP_END;
        ret = ioctl(fd, VIDIOC_TRY_ENCODER_CMD, &cmd);
        if (0 > ret)
            debug_printf("HandleOutbuf TRY_ENCODER_CMD failed %d %s\n",
                    errno, strerror(errno));
        ret = ioctl(fd, VIDIOC_ENCODER_CMD, &cmd);
        if (0 > ret)
            debug_printf("HandleOutbuf ENCODER_CMD failed %d %s\n",
                    errno, strerror(errno));
        // frame_count++;
    }

    /*if (frame_count > n_frames) {
        debug_printf("All source frames send %d\n", n_frames);
        return 0;
    }*/

    memset(buffer.mapped, 0, buffer.length);

    if (ac == 1 || ac % 3 == 0) {
        ac++;
        memcpy((reinterpret_cast<char*>(buffer.mapped)),
                yuv_data,
                format.width * format.height * format.size_num / format.size_den);
    } else {
        ac++;
        debug_printf("read returned 0, exiting handle_outbuf\n");
        return 0;
    }

    ret = QueueBuffer(fd, index, buffer, format);
    frame_count++;

    return ret;
}

int EncoderProcess::HandleCapbuf(int fd, int wrfd, int index,
             struct tienc_buffer buffer,
             struct tienc_format format,
             int write_length,
             char *stream,
             int *stream_len) {
    int ret = 0;

    if (write_length) {
        debug_printf("encoder H264 length is %d\n", write_length);
        memcpy(stream, reinterpret_cast<char*>(buffer.mapped), write_length);
        *stream_len = write_length;
        ret = write(wrfd, buffer.mapped, write_length);
    }
    if (0 > ret)
        debug_printf("write returned error: %d: %s\n", errno,
               strerror(errno));

    ret = QueueBuffer(fd, index, buffer, format);

    return ret;
}

int EncoderProcess::StreamingLoop(void * ptr_encode_info) {
    struct v4l2_buffer buf;
    struct v4l2_plane buf_planes[MAX_PLANES];
    struct pollfd pfd;
    int type, i, ret = 0;
    uint32_t flags = 0;
    struct v4l2_event event;
    EncoderProcess encoder;

    ENCODE_INFO_T *p_encode_info =  (ENCODE_INFO_T *)ptr_encode_info;   // NOLINT

    debug_printf("%s Enter\n", __func__);

    debug_printf("%s ....\n", "aaaaaaaaaaaaaaaaaaaaaa");

    for (i = 0; i < p_encode_info->n_outbuffers; i++)
        encoder.HandleOutbuf(p_encode_info->fd, p_encode_info->rdfd, i, p_encode_info->outbuffers[i],
                             p_encode_info->out_format, p_encode_info->n_frames, p_encode_info->yuv_data);
    debug_printf("%s ....\n", "bbbbbbbbbbbbbbbbbbbbbbb");

    for (i = 0; i < p_encode_info->n_capbuffers; i++)
        encoder.HandleCapbuf(p_encode_info->fd, p_encode_info->wrfd, i, p_encode_info->capbuffers[i],
                             p_encode_info->cap_format, 0, p_encode_info->enc_stream, p_encode_info->stream_len);
    type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
    debug_printf("%s ....\n", "cccccccccccccccccccccccccc");

    ret = ioctl(p_encode_info->fd, VIDIOC_STREAMON, &type);
    if (ret) {
        debug_printf("OUTPUT VIDIOC_STREAMON failed %d: %s\n", errno,
               strerror(errno));
        return ret;
    }
    debug_printf("OUTPUT VIDIOC_STREAMON succeeded\n");

    type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    ret = ioctl(p_encode_info->fd, VIDIOC_STREAMON, &type);
    if (ret) {
        debug_printf("CAPTURE VIDIOC_STREAMON failed %d: %s\n", errno,
               strerror(errno));
        return ret;
    }
    debug_printf("CAPTURE VIDIOC_STREAMON succeeded\n");

    pfd.fd = p_encode_info->fd;
    pfd.events = POLLIN | POLLOUT | POLLPRI;

    while (!(flags & V4L2_BUF_FLAG_LAST)) {
        /* Poll for any event for 100ms */
        pfd.revents = 0;
        ret = poll(&pfd, 1, 15);
        if (0 > ret) {
            debug_printf("poll had an error %d: %s\n",
                   errno, strerror(errno));
        } else if (0 < ret) {
            if (pfd.revents & POLLOUT) {
                while (1) {
                    debug_printf("POLLOUT while\n");
                    memset(&buf, 0, sizeof(buf));
                    memset(&buf_planes, 0, sizeof(buf_planes));
                    buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
                    buf.memory = p_encode_info->out_format.memory;
                    buf.m.planes = buf_planes;
                    buf.length = p_encode_info->out_format.n_planes;
                    ret = ioctl(p_encode_info->fd, VIDIOC_DQBUF, &buf);
                    if (ret < 0) {
                        if (EAGAIN != errno) {
                            debug_printf("OUTPUT VIDIOC_DQBUF failed %d: %s\n",
                                   errno,
                                   strerror(errno));
                        } else {
                            debug_printf("OUTPUT EAGAIN\n");
                            break;
                        }
                    } else {
                        encoder.HandleOutbuf(p_encode_info->fd, p_encode_info->rdfd, buf.index,
                                  p_encode_info->outbuffers[buf.index],
                                  p_encode_info->out_format,
                                  p_encode_info->n_frames, p_encode_info->yuv_data);
                    }
                }
            }
            if (pfd.revents & POLLIN) {
                while (1) {
                    debug_printf("POLLIN while\n");
                    memset(&buf, 0, sizeof(buf));
                    memset(&buf_planes, 0, sizeof(buf_planes));
                    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
                    buf.memory = p_encode_info->cap_format.memory;
                    buf.m.planes = buf_planes;
                    buf.length = p_encode_info->cap_format.n_planes;
                    ret = ioctl(p_encode_info->fd, VIDIOC_DQBUF, &buf);
                    if (ret < 0) {
                        if (EAGAIN != errno) {
                            debug_printf("CAPTURE VIDIOC_DQBUF failed %d: %s\n",
                                   errno,
                                   strerror(errno));
                            if (EPIPE == errno) {
                                debug_printf("Got EPIPE, exiting\n");
                                flags |= V4L2_BUF_FLAG_LAST;
                                break;
                            }
                        } else {
                            debug_printf("CAPTURE EAGAIN\n");
                            break;
                        }
                    } else {
                        encoder.HandleCapbuf(p_encode_info->fd, p_encode_info->wrfd, buf.index,
                                  p_encode_info->capbuffers[buf.index],
                                  p_encode_info->cap_format,
                                  buf.m.planes[0].bytesused,
                                  p_encode_info->enc_stream,
                                  p_encode_info->stream_len);
                        flags = buf.flags;
                        debug_printf("CAPTURE VIDIOC_DQBUF buffer %d flags=%#08x FLAG_LAST=%#08x\n",
                                 buf.index, flags,
                                 V4L2_BUF_FLAG_LAST);
                        if (buf.flags & V4L2_BUF_FLAG_LAST)
                            break;
                    }
                }
            }
            if (pfd.revents & POLLPRI) {
                memset(&event, 0, sizeof(event));
                ret = ioctl(p_encode_info->fd, VIDIOC_DQEVENT, &event);
                if (ret < 0) {
                    debug_printf("VIDIOC_DQEVENT failed %d: %s\n",
                            errno, strerror(errno));
                } else if (event.type == V4L2_EVENT_EOS) {
                    debug_printf("GOT EOS EVENT\n");
                } else {
                    debug_printf("VIDIOC_DQEVENT got unexpected event %d\n",
                           event.type);
                }
            }
        } else {
            break;
        }
    }

    type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
    ret = ioctl(p_encode_info->fd, VIDIOC_STREAMOFF, &type);
    if (ret)
        debug_printf("OUTPUT VIDIOC_STREAMOFF failed %d: %s\n", errno,
               strerror(errno));

    type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    ret = ioctl(p_encode_info->fd, VIDIOC_STREAMOFF, &type);
    if (ret)
        debug_printf("CAPTURE VIDIOC_STREAMOFF failed %d: %s\n", errno,
               strerror(errno));
    debug_printf("%s Exit\n", __func__);

    return 0;
}

int EncoderProcess::CreateMmapBuffer(int fd, struct tienc_buffer *b,
                  enum v4l2_buf_type type, int index,
                  struct tienc_format format) {
    struct v4l2_buffer buf;
    struct v4l2_plane buf_planes[MAX_PLANES];

    debug_printf("%s Enter\n", __func__);

    memset(&buf, 0, sizeof(buf));
    buf.type = type;
    buf.index = index;
    buf.m.planes = buf_planes;
    buf.length = 1;

    if (ioctl(fd, VIDIOC_QUERYBUF, &buf)) {
        debug_printf("VIDIOC_QUERYBUF %s %d failed: %d: %s\n",
               (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) ?
               ("CAPTURE") : ("OUTPUT"), index, errno,
               strerror(errno));
        return -1;
    }

    debug_printf("QUERYBUF %s: buffer %d length (planes)=%d length=%d offset=%d\n",
             (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) ?
             ("CAPTURE") : ("OUTPUT"), index, buf.length,
             buf.m.planes[0].length,
             buf.m.planes[0].data_offset);

    /*
     * Required size for CAPTURE can be more finely tuned by driver.
     * Our calculated size for CAPTURE is a worst case scenario, so allow
     * driver to take priority.
     */
    if ((type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) &&
        (buf.m.planes[0].length < format.size)) {
        debug_printf("Buffer length %d less than required %d\n",
               buf.m.planes[0].length, format.size);
        return -1;
    }

    debug_printf("---map length = %d\n", buf.m.planes[0].length);
    b->mapped = mmap(NULL, buf.m.planes[0].length,
                 PROT_READ | PROT_WRITE, MAP_SHARED,
                 fd, buf.m.planes[0].m.mem_offset);
    b->offset = buf.m.planes[0].m.mem_offset;
    b->length = buf.m.planes[0].length;

    debug_printf("After mmap, buffers[%d].mapped = 0x%p\n", index,
             b->mapped);

    if (MAP_FAILED == b->mapped) {
        debug_printf("mmap of %s buffer failed %d: %s\n",
               (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) ?
               ("CAPTURE") : ("OUTPUT"), errno,
               strerror(errno));
        return -1;
    }
    return 0;
}

int EncoderProcess::AllocBufs(int fd, int drmfd, int *n_buffers,
              struct tienc_buffer buffers[], enum v4l2_buf_type type,
              struct tienc_format format) {
    int i, max, ret = 0;
    struct v4l2_requestbuffers reqbuf = {0};
    debug_printf("%s Enter\n", __func__);

    reqbuf.count = *n_buffers;
    reqbuf.type = type;
    reqbuf.memory = format.memory;

    ret = ioctl(fd, VIDIOC_REQBUFS, &reqbuf);
    if (ret) {
        debug_printf("VIDIOC_REQBUF %s failed %d: %s\n",
               (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) ?
               ("CAPTURE") : ("OUTPUT"), errno, strerror(errno));
        return ret;
    }

    debug_printf("After VIDIOC_REQBUFS %s getting buf_cnt %d\n",
             (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) ?
             ("CAPTURE") : ("OUTPUT"), reqbuf.count);

    *n_buffers = reqbuf.count;

    if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type) {
        max = MAX_CAPBUFS;
    } else if (V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE == type) {
        max = MAX_OUTBUFS;
    } else {
        debug_printf("Invalid type %#08x chosen\n", type);
        return -1;
    }

    if (max < *n_buffers) {
        debug_printf("Driver returned more %s buffers %d than MAX %d\n",
               (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) ?
               ("CAPTURE") : ("OUTPUT"), *n_buffers, max);
        return -1;
    }

    for (i = 0; i < *n_buffers; i++) {
        if (format.memory == V4L2_MEMORY_MMAP) {
            ret = CreateMmapBuffer(fd, &buffers[i], type, i, format);
            if (ret)
                return ret;
        } else if (format.memory == V4L2_MEMORY_DMABUF) {
#ifdef DRM
            ret = create_drm_buffer(drmfd, &buffers[i], format);
            if (ret)
                return ret;
#endif
        }
    }

    return 0;
}

void EncoderProcess::DeallocBufs(int fd, int drmfd, int *n_buffers, struct tienc_buffer buffers[],
             struct tienc_format format) {
    int i, ret = 0;
#ifdef DRM
    struct drm_mode_destroy_dumb gem_destroy;
#endif

    debug_printf("%s Enter\n", __func__);

    for (i = 0; i < *n_buffers; i++) {
        if (NULL == buffers[i].mapped) {
            debug_printf("buffer %d not mapped, skip munmap\n", i);
            continue;
        }
        debug_printf("munmap buffer %d mapped=0x%p length =%d\n",
                 i, buffers[i].mapped, buffers[i].length);
        ret = munmap(buffers[i].mapped, buffers[i].length);
        if (ret)
            debug_printf("munmap failed for buffer %d. %d: %s\n",
                   i, errno, strerror(errno));
        buffers[i].mapped = NULL;

#ifdef DRM
        if (format.memory == V4L2_MEMORY_DMABUF) {
            memset(&gem_destroy, 0, sizeof(gem_destroy));
            gem_destroy.handle = buffers[i].bo_handle;
            ret = ioctl(drmfd, DRM_IOCTL_MODE_DESTROY_DUMB,
                    &gem_destroy);
            if (ret)
                debug_printf("DRM_IOCTL_MODE_DESTROY_DUMB failed for buffer %d. %d: %s\n",
                       i, errno, strerror(errno));
        }
#endif
    }
}

void EncoderProcess::PrintV4l2Format(struct v4l2_format *fmt) {
    int i;

    debug_printf("type = %d\n", fmt->type);
    debug_printf("width=%d height=%d\n", fmt->fmt.pix_mp.width,
             fmt->fmt.pix_mp.height);
    debug_printf("pixelformat=%d\n", fmt->fmt.pix_mp.pixelformat);
    debug_printf("field=%d colorspace=%d\n", fmt->fmt.pix_mp.field,
             fmt->fmt.pix_mp.colorspace);
    for (i = 0; i < fmt->fmt.pix_mp.num_planes; i++) {
        debug_printf("plane_fmt[%d].sizeimage=%d\n", i,
                 fmt->fmt.pix_mp.plane_fmt[i].sizeimage);
        debug_printf("plane_fmt[%d].bytesperline=%d\n", i,
                 fmt->fmt.pix_mp.plane_fmt[i].bytesperline);
    }
    debug_printf("num_planes=%d flags=%d\n", fmt->fmt.pix_mp.num_planes,
            fmt->fmt.pix_mp.flags);
}

void EncoderProcess::GetFormat(int fd, struct tienc_format format) {
    struct v4l2_format fmt = {0};

    debug_printf("%s Enter\n", __func__);

    fmt.type = format.type;

    if (ioctl(fd, VIDIOC_G_FMT, &fmt)) {
        debug_printf("VIDIOC_G_FMT %s errno %d: %s\n",
               (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == format.type) ?
               ("CAPTURE") : ("OUTPUT"), errno, strerror(errno));
        return;
    }

    debug_printf("%s printing returned v4l2_format\n", __func__);
    PrintV4l2Format(&fmt);
}

int EncoderProcess::TryFormat(int fd, struct tienc_format format) {
    struct v4l2_format fmt = {0};
    int ret = 0;

    debug_printf("%s Enter\n", __func__);

    fmt.type = format.type;
    fmt.fmt.pix_mp.width = format.width;
    fmt.fmt.pix_mp.height = format.height;
    fmt.fmt.pix_mp.pixelformat = format.fourcc;
    fmt.fmt.pix_mp.field = V4L2_FIELD_NONE;
    fmt.fmt.pix_mp.colorspace = V4L2_COLORSPACE_DEFAULT;
    fmt.fmt.pix_mp.plane_fmt[0].sizeimage = format.size;
    fmt.fmt.pix_mp.plane_fmt[0].bytesperline = format.stride;
    fmt.fmt.pix_mp.num_planes = format.n_planes;

    ret = ioctl(fd, VIDIOC_TRY_FMT, &fmt);
    if (ret) {
        debug_printf("VIDIOC_TRY_FMT %s errno %d: %s\n",
               (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == format.type) ?
               ("CAPTURE") : ("OUTPUT"), errno, strerror(errno));
        return ret;
    }

    debug_printf("%s printing returned v4l2_format\n", __func__);
    PrintV4l2Format(&fmt);
    return 0;
}

int EncoderProcess::SetFormat(int fd, struct tienc_format format) {
    struct v4l2_format fmt = {0};
    int ret = 0;

    debug_printf("%s Enter\n", __func__);

    fmt.type = format.type;
    fmt.fmt.pix_mp.width = format.width;
    fmt.fmt.pix_mp.height = format.height;
    fmt.fmt.pix_mp.pixelformat = format.fourcc;
    fmt.fmt.pix_mp.field = V4L2_FIELD_NONE;
    fmt.fmt.pix_mp.colorspace = V4L2_COLORSPACE_DEFAULT;
    fmt.fmt.pix_mp.plane_fmt[0].sizeimage = format.size;
    fmt.fmt.pix_mp.plane_fmt[0].bytesperline = format.stride;
    fmt.fmt.pix_mp.num_planes = format.n_planes;

    ret = ioctl(fd, VIDIOC_S_FMT, &fmt);
    if (ret) {
        debug_printf("VIDIOC_S_FMT %s errno %d: %s\n",
               (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == format.type) ?
               ("CAPTURE") : ("OUTPUT"), errno, strerror(errno));
        return ret;
    }

    debug_printf("%s printing returned v4l2_format\n", __func__);
    PrintV4l2Format(&fmt);
    return 0;
}

void EncoderProcess::GetParameters(int fd) {
    struct v4l2_streamparm parm = {0};

    parm.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
    parm.parm.output.capability = V4L2_CAP_TIMEPERFRAME;

    if (ioctl(fd, VIDIOC_G_PARM, &parm)) {
        debug_printf("VIDIOC_G_PARM failed %d %s\n", errno, strerror(errno));
        return;
    }

    debug_printf("VIDIOC_G_PARM got back framerate %d / %d = %d\n",
             parm.parm.output.timeperframe.denominator,
             parm.parm.output.timeperframe.numerator,
             (parm.parm.output.timeperframe.denominator /
              parm.parm.output.timeperframe.numerator));

    return;
}

int EncoderProcess::SetParameters(int fd, int num, int den) {
    struct v4l2_streamparm parm = {0};

    parm.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
    parm.parm.output.capability = V4L2_CAP_TIMEPERFRAME;
    parm.parm.output.timeperframe.numerator = num;
    parm.parm.output.timeperframe.denominator = den;

    if (ioctl(fd, VIDIOC_S_PARM, &parm)) {
        debug_printf("VIDIOC_S_PARM failed %d %s\n", errno, strerror(errno));
        return 1;
    }

    debug_printf("VIDIOC_S_PARM got back framerate %d / %d = %d\n",
             parm.parm.output.timeperframe.denominator,
             parm.parm.output.timeperframe.numerator,
             (parm.parm.output.timeperframe.denominator /
              parm.parm.output.timeperframe.numerator));

    return 0;
}

void EncoderProcess::QueryCtrls(int fd) {
    struct v4l2_queryctrl ctrl;
    struct v4l2_query_ext_ctrl ext_ctrl;

    debug_printf("%s Enter\n", __func__);

    debug_printf("%s Enumerating all non-compound\n", __func__);
    memset(&ctrl, 0, sizeof(ctrl));
    ctrl.id = 0;
    ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
    while (!ioctl(fd, VIDIOC_QUERYCTRL, &ctrl)) {
        debug_printf("%s Got ctrl.id=%#08x type=%d\n",
               __func__, ctrl.id, ctrl.type);
        debug_printf("%s\tname=%s minimum=%d maximum=%d\n",
               __func__, ctrl.name, ctrl.minimum, ctrl.maximum);
        debug_printf("%s\tstep=%d default_value=%d\n",
               __func__, ctrl.step, ctrl.default_value);
        debug_printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
        ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
    }
    debug_printf("%s Enumerating all non-compound ended errno=%d %s\n",
            __func__, errno, strerror(errno));

    debug_printf("%s Enumerating all non-compound ext\n", __func__);
    memset(&ext_ctrl, 0, sizeof(ext_ctrl));
    ext_ctrl.id = 0;
    ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
    while (!ioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ext_ctrl)) {
        debug_printf("%s Got ext_ctrl.id=%#08x type=%d\n",
               __func__, ext_ctrl.id, ext_ctrl.type);
        debug_printf("%s\tname=%s minimum=%lld maximum=%lld\n",
               __func__, ext_ctrl.name, ext_ctrl.minimum,
               ext_ctrl.maximum);
        debug_printf("%s\tstep=%lld default_value=%lld\n",
               __func__, ext_ctrl.step, ext_ctrl.default_value);
        debug_printf("%s\telem_size=%d elems=%d nr_of_dims=%d\n",
               __func__, ext_ctrl.elem_size, ext_ctrl.elems,
               ext_ctrl.nr_of_dims);
        debug_printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
        ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
    }
    debug_printf("%s Enumerating all non-compound ext ended errno=%d %s\n",
            __func__, errno, strerror(errno));

    debug_printf("%s Enumerating all compound ext\n", __func__);
    memset(&ext_ctrl, 0, sizeof(ext_ctrl));
    ext_ctrl.id = 0;
    ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_COMPOUND;
    while (!ioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ext_ctrl)) {
        debug_printf("%s Got ext_ctrl.id=%#08x type=%d\n",
               __func__, ext_ctrl.id, ext_ctrl.type);
        debug_printf("%s\tname=%s minimum=%lld maximum=%lld\n",
               __func__, ext_ctrl.name, ext_ctrl.minimum,
               ext_ctrl.maximum);
        debug_printf("%s\tstep=%lld default_value=%lld\n",
               __func__, ext_ctrl.step, ext_ctrl.default_value);
        debug_printf("%s\telem_size=%d elems=%d nr_of_dims=%d\n",
               __func__, ext_ctrl.elem_size, ext_ctrl.elems,
               ext_ctrl.nr_of_dims);
        debug_printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
        ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_COMPOUND;
    }
    debug_printf("%s Enumerating all compound ext ended errno=%d %s\n",
           __func__, errno, strerror(errno));

    debug_printf("%s Enumerating all controls ext\n", __func__);
    memset(&ext_ctrl, 0, sizeof(ext_ctrl));
    ext_ctrl.id = 0;
    ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
    ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_COMPOUND;
    while (!ioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ext_ctrl)) {
        debug_printf("%s Got ext_ctrl.id=%#08x type=%d\n",
               __func__, ext_ctrl.id, ext_ctrl.type);
        debug_printf("%s\tname=%s minimum=%lld maximum=%lld\n",
               __func__, ext_ctrl.name, ext_ctrl.minimum,
               ext_ctrl.maximum);
        debug_printf("%s\tstep=%lld default_value=%lld\n",
               __func__, ext_ctrl.step, ext_ctrl.default_value);
        debug_printf("%s\telem_size=%d elems=%d nr_of_dims=%d\n",
               __func__, ext_ctrl.elem_size, ext_ctrl.elems,
               ext_ctrl.nr_of_dims);
        debug_printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
        ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
        ext_ctrl.id |= V4L2_CTRL_FLAG_NEXT_COMPOUND;
    }
    debug_printf("%s Enumerating all controls ext ended errno=%d %s\n",
           __func__, errno, strerror(errno));

    /* Query individual controls */
    memset(&ctrl, 0, sizeof(ctrl));
    ctrl.id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
    if (ioctl(fd, VIDIOC_QUERYCTRL, &ctrl)) {
        debug_printf("%s error querying V4L2_CID_MPEG_VIDEO_GOP_SIZE errno=%d %s\n",
               __func__, errno, strerror(errno));
    } else {
        debug_printf("%s Got ctrl.id=%#08x type=%d\n",
               __func__, ctrl.id, ctrl.type);
        debug_printf("%s\tname=%s minimum=%d maximum=%d\n",
               __func__, ctrl.name, ctrl.minimum, ctrl.maximum);
        debug_printf("%s\tstep=%d default_value=%d\n",
               __func__, ctrl.step, ctrl.default_value);
        debug_printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
    }

    memset(&ctrl, 0, sizeof(ctrl));
    ctrl.id = V4L2_CID_MPEG_VIDEO_BITRATE;
    if (ioctl(fd, VIDIOC_QUERYCTRL, &ctrl)) {
        debug_printf("%s error querying V4L2_CID_MPEG_VIDEO_BITRATE errno=%d %s\n",
               __func__, errno, strerror(errno));
    } else {
        debug_printf("%s Got ctrl.id=%#08x type=%d\n",
               __func__, ctrl.id, ctrl.type);
        debug_printf("%s\tname=%s minimum=%d maximum=%d\n",
               __func__, ctrl.name, ctrl.minimum, ctrl.maximum);
        debug_printf("%s\tstep=%d default_value=%d\n",
               __func__, ctrl.step, ctrl.default_value);
        debug_printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
    }

    memset(&ctrl, 0, sizeof(ctrl));
    ctrl.id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
    if (ioctl(fd, VIDIOC_QUERYCTRL, &ctrl)) {
        debug_printf("%s error querying V4L2_CID_MPEG_VIDEO_H264_I_PERIOD errno=%d %s\n",
               __func__, errno, strerror(errno));
    } else {
        debug_printf("%s Got ctrl.id=%#08x type=%d\n",
               __func__, ctrl.id, ctrl.type);
        debug_printf("%s\tname=%s minimum=%d maximum=%d\n",
               __func__, ctrl.name, ctrl.minimum, ctrl.maximum);
        debug_printf("%s\tstep=%d default_value=%d\n",
               __func__, ctrl.step, ctrl.default_value);
        debug_printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
    }

    /* Query individual extended controls */
    memset(&ext_ctrl, 0, sizeof(ext_ctrl));
    ext_ctrl.id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
    if (ioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ext_ctrl)) {
        debug_printf("%s error querying ext V4L2_CID_MPEG_VIDEO_GOP_SIZE errno=%d %s\n",
               __func__, errno, strerror(errno));
    } else {
        debug_printf("%s Got ext_ctrl.id=%#08x type=%d\n",
               __func__, ext_ctrl.id, ext_ctrl.type);
        debug_printf("%s\tname=%s minimum=%lld maximum=%lld\n",
               __func__, ext_ctrl.name, ext_ctrl.minimum,
               ext_ctrl.maximum);
        debug_printf("%s\tstep=%lld default_value=%lld\n",
               __func__, ext_ctrl.step, ext_ctrl.default_value);
        debug_printf("%s\telem_size=%d elems=%d nr_of_dims=%d\n",
               __func__, ext_ctrl.elem_size, ext_ctrl.elems,
               ext_ctrl.nr_of_dims);
        debug_printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
    }

    memset(&ext_ctrl, 0, sizeof(ext_ctrl));
    ext_ctrl.id = V4L2_CID_MPEG_VIDEO_BITRATE;
    if (ioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ext_ctrl)) {
        debug_printf("%s error querying ext V4L2_CID_MPEG_VIDEO_BITRATE errno=%d %s\n",
               __func__, errno, strerror(errno));
    } else {
        debug_printf("%s Got ext_ctrl.id=%#08x type=%d\n",
               __func__, ext_ctrl.id, ext_ctrl.type);
        debug_printf("%s\tname=%s minimum=%lld maximum=%lld\n",
               __func__, ext_ctrl.name, ext_ctrl.minimum,
               ext_ctrl.maximum);
        debug_printf("%s\tstep=%lld default_value=%lld\n",
               __func__, ext_ctrl.step, ext_ctrl.default_value);
        debug_printf("%s\telem_size=%d elems=%d nr_of_dims=%d\n",
               __func__, ext_ctrl.elem_size, ext_ctrl.elems,
               ext_ctrl.nr_of_dims);
        debug_printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
    }

    memset(&ext_ctrl, 0, sizeof(ext_ctrl));
    ext_ctrl.id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
    if (ioctl(fd, VIDIOC_QUERY_EXT_CTRL, &ext_ctrl)) {
        debug_printf("%s error querying ext V4L2_CID_MPEG_VIDEO_H264_I_PERIOD errno=%d %s\n",
               __func__, errno, strerror(errno));
    } else {
        debug_printf("%s Got ext_ctrl.id=%#08x type=%d\n",
               __func__, ext_ctrl.id, ext_ctrl.type);
        debug_printf("%s\tname=%s minimum=%lld maximum=%lld\n",
               __func__, ext_ctrl.name, ext_ctrl.minimum,
               ext_ctrl.maximum);
        debug_printf("%s\tstep=%lld default_value=%lld\n",
               __func__, ext_ctrl.step, ext_ctrl.default_value);
        debug_printf("%s\telem_size=%d elems=%d nr_of_dims=%d\n",
               __func__, ext_ctrl.elem_size, ext_ctrl.elems,
               ext_ctrl.nr_of_dims);
        debug_printf("%s\tflags=%#08x\n", __func__, ctrl.flags);
    }
}

void EncoderProcess::GetCtrls(int fd) {
    struct v4l2_ext_controls ctrls;
    struct v4l2_ext_control controls[3];
    unsigned int i;

    memset(&ctrls, 0, sizeof(ctrls));
    memset(controls, 0, sizeof(controls));

    ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
    ctrls.count = 3;
    ctrls.controls = controls;

    controls[0].id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
    controls[1].id = V4L2_CID_MPEG_VIDEO_BITRATE;
    controls[2].id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;

    if (ioctl(fd, VIDIOC_G_EXT_CTRLS, &ctrls)) {
        debug_printf("%s error getting VIDIOC_G_EXT_CTRLS errno=%d %s\n",
               __func__, errno, strerror(errno));
        return;
    }

    debug_printf("%s got %d values back from VIDIOC_G_EXT_CTRLS\n",
             __func__, ctrls.count);
    for (i = 0; i < ctrls.count; i++) {
        debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
                 __func__, i, controls[i].id,
                 controls[i].size, controls[i].value);
    }
}

void EncoderProcess::TryCtrls(int fd, int bitrate, int gop_size, int i_period) {
    struct v4l2_ext_controls ctrls;
    struct v4l2_ext_control controls[3];
    unsigned int i;

    memset(&ctrls, 0, sizeof(ctrls));
    memset(controls, 0, sizeof(controls));

    ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
    ctrls.count = 3;
    ctrls.controls = controls;

    controls[0].id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
    controls[0].value = gop_size;
    controls[1].id = V4L2_CID_MPEG_VIDEO_BITRATE;
    controls[1].value = bitrate;
    controls[2].id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
    controls[2].value = i_period;

    if (ioctl(fd, VIDIOC_TRY_EXT_CTRLS, &ctrls)) {
        debug_printf("%s error getting VIDIOC_TRY_EXT_CTRLS error_idx=%d errno=%d %s\n",
               __func__, ctrls.error_idx, errno, strerror(errno));
        debug_printf("%s Successful commands TRYed:\n", __func__);
        for (i = 0; i < ctrls.error_idx; i++) {
            debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
                     __func__, i, controls[i].id,
                     controls[i].size, controls[i].value);
        }
    } else {
        debug_printf("%s VIDIOC_TRY_EXT_CTRLS succeeded\n", __func__);
        for (i = 0; i < ctrls.count; i++) {
            debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
                     __func__, i, controls[i].id,
                     controls[i].size, controls[i].value);
        }
    }

#ifdef DEBUG
    debug_printf("%s Testing TRY setting DEF_VAL, EINVAL expected\n", __func__);
    memset(&ctrls, 0, sizeof(ctrls));
    memset(controls, 0, sizeof(controls));

    ctrls.which = V4L2_CTRL_WHICH_DEF_VAL;
    ctrls.count = 3;
    ctrls.controls = controls;

    controls[0].id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
    controls[0].value = 0;
    controls[1].id = V4L2_CID_MPEG_VIDEO_BITRATE;
    controls[1].value = 0;
    controls[2].id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
    controls[2].value = 0;

    if (ioctl(fd, VIDIOC_TRY_EXT_CTRLS, &ctrls)) {
        debug_printf("%s error getting VIDIOC_TRY_EXT_CTRLS error_idx=%d errno=%d %s\n",
               __func__, ctrls.error_idx, errno, strerror(errno));
        for (i = 0; i < ctrls.error_idx; i++) {
            debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
                     __func__, i, controls[i].id,
                     controls[i].size, controls[i].value);
        }
    } else {
        debug_printf("%s VIDIOC_TRY_EXT_CTRLS succeeded\n", __func__);
        for (i = 0; i < ctrls.count; i++) {
            debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
                     __func__, i, controls[i].id,
                     controls[i].size, controls[i].value);
        }
    }

    debug_printf("%s Testing TRY setting controls[0] to invalid, ERANGE expected with idx=0\n", __func__);
    memset(&ctrls, 0, sizeof(ctrls));
    memset(controls, 0, sizeof(controls));

    ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
    ctrls.count = 3;
    ctrls.controls = controls;

    controls[0].id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
    controls[0].value = 0;
    controls[1].id = V4L2_CID_MPEG_VIDEO_BITRATE;
    controls[1].value = 0;
    controls[2].id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
    controls[2].value = 0;

    if (ioctl(fd, VIDIOC_TRY_EXT_CTRLS, &ctrls)) {
        debug_printf("%s error getting VIDIOC_TRY_EXT_CTRLS error_idx=%d errno=%d %s\n",
               __func__, ctrls.error_idx, errno, strerror(errno));
        for (i = 0; i < ctrls.error_idx; i++) {
            debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
                     __func__, i, controls[i].id,
                     controls[i].size, controls[i].value);
        }
    } else {
        debug_printf("%s VIDIOC_TRY_EXT_CTRLS succeeded\n", __func__);
        for (i = 0; i < ctrls.count; i++) {
            debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
                     __func__, i, controls[i].id,
                     controls[i].size, controls[i].value);
        }
    }

    debug_printf("%s Testing TRY setting controls[1] to invalid, ERANGE expected with idx=1\n", __func__);
    memset(&ctrls, 0, sizeof(ctrls));
    memset(controls, 0, sizeof(controls));

    ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
    ctrls.count = 3;
    ctrls.controls = controls;

    controls[0].id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
    controls[0].value = 5;
    controls[1].id = V4L2_CID_MPEG_VIDEO_BITRATE;
    controls[1].value = 0;
    controls[2].id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
    controls[2].value = 0;

    if (ioctl(fd, VIDIOC_TRY_EXT_CTRLS, &ctrls)) {
        debug_printf("%s error getting VIDIOC_TRY_EXT_CTRLS error_idx=%d errno=%d %s\n",
               __func__, ctrls.error_idx, errno, strerror(errno));
        for (i = 0; i < ctrls.error_idx; i++) {
            debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
                     __func__, i, controls[i].id,
                     controls[i].size, controls[i].value);
        }
    } else {
        debug_printf("%s VIDIOC_TRY_EXT_CTRLS succeeded\n", __func__);
        for (i = 0; i < ctrls.count; i++) {
            debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
                     __func__, i, controls[i].id,
                     controls[i].size, controls[i].value);
        }
    }

    debug_printf("%s Testing TRY setting controls[2] to invalid, ERANGE expected with idx=2\n", __func__);
    memset(&ctrls, 0, sizeof(ctrls));
    memset(controls, 0, sizeof(controls));

    ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
    ctrls.count = 3;
    ctrls.controls = controls;

    controls[0].id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
    controls[0].value = 5;
    controls[1].id = V4L2_CID_MPEG_VIDEO_BITRATE;
    controls[1].value = 1000000;
    controls[2].id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
    controls[2].value = 0;

    if (ioctl(fd, VIDIOC_TRY_EXT_CTRLS, &ctrls)) {
        debug_printf("%s error getting VIDIOC_TRY_EXT_CTRLS error_idx=%d errno=%d %s\n",
               __func__, ctrls.error_idx, errno, strerror(errno));
        for (i = 0; i < ctrls.error_idx; i++) {
            debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
                     __func__, i, controls[i].id,
                     controls[i].size, controls[i].value);
        }
    } else {
        debug_printf("%s VIDIOC_TRY_EXT_CTRLS succeeded\n", __func__);
        for (i = 0; i < ctrls.count; i++) {
            debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
                     __func__, i, controls[i].id,
                     controls[i].size, controls[i].value);
        }
    }
#endif
}

void EncoderProcess::SetCtrls(int fd, int bitrate, int gop_size, int i_period) {
    struct v4l2_ext_controls ctrls;
    struct v4l2_ext_control controls[3];
    unsigned int i;

    memset(&ctrls, 0, sizeof(ctrls));
    memset(controls, 0, sizeof(controls));

    ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
    ctrls.count = 3;
    ctrls.controls = controls;

    controls[0].id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
    controls[0].value = gop_size;
    controls[1].id = V4L2_CID_MPEG_VIDEO_BITRATE;
    controls[1].value = bitrate;
    controls[2].id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD;
    controls[2].value = i_period;

    if (ioctl(fd, VIDIOC_S_EXT_CTRLS, &ctrls)) {
        debug_printf("%s error getting VIDIOC_S_EXT_CTRLS error_idx=%d errno=%d %s\n",
               __func__, ctrls.error_idx, errno, strerror(errno));
        if (ctrls.error_idx == ctrls.count) {
            debug_printf("%s S_EXT_CTRLS failed during pre-check\n",
                   __func__);
        } else {
            debug_printf("%s S_EXT_CTRLS failed during set. Controls set before failure:\n",
                   __func__);
            for (i = 0; i < ctrls.error_idx; i++) {
                debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
                       __func__, i, controls[i].id,
                       controls[i].size, controls[i].value);
            }
        }
    } else {
        debug_printf("%s VIDIOC_S_EXT_CTRLS succeeded\n", __func__);
        for (i = 0; i < ctrls.count; i++) {
            debug_printf("%s controls[%d] id=%#08x got size=%d value=%d\n",
                     __func__, i, controls[i].id,
                     controls[i].size, controls[i].value);
        }
    }
}

int EncoderProcess::SubscribeEvents(int fd) {
    struct v4l2_event_subscription sub;

    sub.type = V4L2_EVENT_EOS;
    debug_printf("Calling V4L2 IOCTL VIDIOC_SUBSCRIBE_EVENT\n");
    if (ioctl(fd, VIDIOC_SUBSCRIBE_EVENT, &sub)) {
        debug_printf("Failed to subscribe to events: err: %d %s\n",
                errno, strerror(errno));
        return 1;
    }

    sub.type = V4L2_EVENT_EOS;
    debug_printf("Calling V4L2 IOCTL VIDIOC_SUBSCRIBE_EVENT\n");
    if (ioctl(fd, VIDIOC_SUBSCRIBE_EVENT, &sub)) {
        debug_printf("Failed to subscribe to events: err: %d %s\n",
                errno, strerror(errno));
        return 1;
    }

    return 0;
}

void EncoderProcess::UnsubscribeEvents(int fd) {
    struct v4l2_event_subscription sub;

    sub.type = V4L2_EVENT_ALL;
    debug_printf("Calling V4L2 IOCTL VIDIOC_UNSUBSCRIBE_EVENT\n");
    if (ioctl(fd, VIDIOC_UNSUBSCRIBE_EVENT, &sub))
        debug_printf("Failed to unsubscribe from events: err: %d %s\n",
                errno, strerror(errno));

    return;
}

int EncoderProcess::QueryDevice(int fd) {
    struct v4l2_capability cap = {0};
    struct v4l2_fmtdesc desc = {0};
    struct v4l2_frmsizeenum frmsize = {0};
    int ret = 0;

    debug_printf("%s Enter\n", __func__);

    ret = ioctl(fd, VIDIOC_QUERYCAP, &cap);
    if (ret) {
        debug_printf("VIDIOC_QUERYCAP failed\n");
        return -1;
    }

    debug_printf("Querycaps: fd=%#x driver=%s card=%s bus_info=%s\n",
             fd, cap.driver, cap.card, cap.bus_info);
    debug_printf("Querycaps: device_caps=%#08x capabilities=%#08x\n",
             cap.device_caps, cap.capabilities);

    desc.index = 0;
    desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;

    debug_printf("Calling VIDIOC_ENUM_FMT on CAPTURE\n");
    while ((ret = ioctl(fd, VIDIOC_ENUM_FMT, &desc)) == 0) {
        debug_printf("desc.index = %d, pixelformat = %c%c%c%c, desciption = %s\n",
                 desc.index, desc.pixelformat & 0xff,
                 (desc.pixelformat >> 8) & 0xff,
                 (desc.pixelformat >> 16) & 0xff,
                 (desc.pixelformat >> 24) & 0xff,
                 desc.description);
        desc.index++;
    }

    desc.index = 0;
    desc.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;

    debug_printf("Calling VIDIOC_ENUM_FMT on OUTPUT\n");
    while ((ret = ioctl(fd, VIDIOC_ENUM_FMT, &desc)) == 0) {
        debug_printf("desc.index = %d, pixelformat = %c%c%c%c, desciption = %s\n",
                 desc.index, desc.pixelformat & 0xff,
                 (desc.pixelformat >> 8) & 0xff,
                 (desc.pixelformat >> 16) & 0xff,
                 (desc.pixelformat >> 24) & 0xff,
                 desc.description);
        desc.index++;
    }

    frmsize.index = 0;

    debug_printf("Calling VIDIOC_ENUM_FRAMESIZES\n");
    ret = ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &frmsize);
    if (ret) {
        debug_printf("VIDIOC_ENUM_FRAMESIZES failed %d %s\n",
               errno, strerror(errno));
    } else if (frmsize.type == V4L2_FRMSIZE_TYPE_DISCRETE) {
        debug_printf("VIDIOC_ENUM_FRAMESIZES got DISCRETE\n");
        debug_printf("frmsizes[%d] width=%d height=%d\n", frmsize.index,
                 frmsize.discrete.width,
                 frmsize.discrete.height);
        frmsize.index++;
        while (ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &frmsize) == 0) {
            debug_printf("frmsizes[%d] width=%d height=%d\n",
                     frmsize.index,
                     frmsize.discrete.width,
                     frmsize.discrete.height);
            frmsize.index++;
        }
    } else {
        debug_printf("VIDIOC_ENUM_FRAMESIZES got %s\n",
                 ((frmsize.type == V4L2_FRMSIZE_TYPE_CONTINUOUS) ?
                  "CONTINUOUS" : "STEPWISE"));
        debug_printf("frmsizes min_width=%d max_width=%d step_width=%d\n",
                 frmsize.stepwise.min_width,
                 frmsize.stepwise.max_width,
                 frmsize.stepwise.step_width);
        debug_printf("frmsizes min_height=%d max_height=%d step_height=%d\n",
                 frmsize.stepwise.min_height,
                 frmsize.stepwise.max_height,
                 frmsize.stepwise.step_height);
    }
    return 0;
}

int EncoderProcess::CheckFramerate(int fd, int *frameival_num, int *frameival_den) {
    return 0;
}

int EncoderProcess::GetNFrames(int rdfd, int frame_size) {
    int size, ret;;
    debug_printf("Discovering number of frames in file\n");
    /*
     * Seek to end of file to get size, then move file offset
     * back to beginning of file
     */
    size = lseek(rdfd, 0, SEEK_END);
    lseek(rdfd, 0, SEEK_SET);

    ret = size / frame_size;

    if (size != (frame_size * ret)) {
        debug_printf("Input file does not contain a whole number of frames\n");
        return -1;
    }
    if (0 == ret) {
        debug_printf("Input file contains no frames\n");
        return -1;
    }

    debug_printf("Found %d frames in input file\n", ret);
    return ret;
}

void EncoderProcess::FindFormat(char name[], struct tienc_format *format,
            const struct tienc_format formats[], int formats_size) {
    int i;

    for (i = 0; i < formats_size; i++) {
        if (0 == strcmp(name, formats[i].name)) {
            debug_printf("Found format %s\n", name);
            *format = formats[i];
            break;
        }
    }

    if (i >= formats_size) {
        debug_printf("No format found. Defaulting to %s\n",
               formats[0].name);
        *format = formats[0];
    }
}

void EncoderProcess::CalculateFormats(char format_name[], char codec_name[],
                 struct tienc_format *out_format,
                 struct tienc_format *cap_format,
                 int width, int height) {
    FindFormat(format_name, out_format, out_formats,
            (sizeof(out_formats) / sizeof(out_formats[0])));
    FindFormat(codec_name, cap_format, cap_formats,
            (sizeof(cap_formats) / sizeof(cap_formats[0])));

    /* Fill user supplied and calculated values */
    out_format->width = width;
    out_format->height = height;
    out_format->stride = ALIGN(width * out_format->bytes_pp, HW_ALIGN);
    out_format->size = ((out_format->stride * height) *
                out_format->size_num) / out_format->size_den;
    debug_printf("--- out_format stride=%d size=%d w:%d h:%d---\n",
                out_format->stride, out_format->size, width, height);

    cap_format->width = width;
    cap_format->height = height;
    cap_format->stride = width;
    cap_format->size = cap_format->stride * height;
    debug_printf("--- cap_format stride=%d size=%d w:%d h:%d---\n",
                cap_format->stride, cap_format->size, width, height);
}

#ifdef DRMMODE
int find_drm_device(char *drm_path) {
    const char *drm_name = "cardx";
    char name[256] = "";
    DIR *d;
    struct dirent *dir;
    drmModeResPtr res;
    int fd;

    d = opendir(drm_path);
    if (!d) {
        debug_printf("Failed to open drm device path %s %d %s\n", drm_path,
               errno, strerror(errno));
        return -1;
    }

    while ((dir = readdir(d)) != NULL) {
        if (strncmp(dir->d_name, drm_name, 4) == 0) {
            strncpy(name, drm_path, sizeof(name));
            strncat(name, dir->d_name, sizeof(name));
            fd = open(name, O_CLOEXEC | O_RDWR);
            if (fd < 0) {
                debug_printf("Failed to open drm device %s %d %s\n",
                       name, errno, strerror(errno));
                continue;
            }

            res = drmModeGetResources(fd);
            if (res && res->count_crtcs > 0 &&
                res->count_connectors > 0 &&
                res->count_encoders > 0) {
                debug_printf("No drm device specified, using %s\n",
                         name);
                break;
            }
            close(fd);
            fd = -1;
        }
    }

    if (fd < 0)
        debug_printf("Failed to find a drm device in %s\n", drm_path);

    return fd;
}
#endif

#ifdef DRM
static int create_drm_buffer(int drmfd, struct tienc_buffer *b,
                 struct tienc_format format) {
    struct drm_mode_create_dumb gem;
    struct drm_prime_handle prime;
    struct drm_mode_map_dumb gem_map;
    int ret;

    memset(&gem, 0, sizeof gem);
    gem.width = format.stride;
    gem.height = (format.height * format.size_num) / format.size_den;
    gem.bpp = 8 * format.bytes_pp;

    ret = ioctl(drmfd, DRM_IOCTL_MODE_CREATE_DUMB, &gem);
    if (ret) {
        debug_printf("DRM_IOCTL_MODE_CREATE_DUMB failed %d: %s\n",
               errno, strerror(errno));
        return ret;
    }

    b->bo_handle = gem.handle;

    memset(&prime, 0, sizeof prime);
    prime.handle = b->bo_handle;

    ret = ioctl(drmfd, DRM_IOCTL_PRIME_HANDLE_TO_FD, &prime);
    if (ret) {
        debug_printf("DRM_IOCTL_PRIME_HANDLE_TO_FD failed %d: %s\n",
               errno, strerror(errno));
        return ret;
    }
    b->dbuf_fd = prime.fd;

    memset(&gem_map, 0, sizeof(gem_map));
    gem_map.handle = gem.handle;

    ret = ioctl(drmfd, DRM_IOCTL_MODE_MAP_DUMB, &gem_map);
    if (ret) {
        debug_printf("DRM_IOCTL_MODE_MAP_DUMB failed %d: %s\n",
               errno, strerror(errno));
        return ret;
    }

    if (format.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
        b->mapped = mmap(NULL, (size_t)gem.size, PROT_READ, MAP_SHARED, drmfd, gem_map.offset);     // NOLINT
    } else if (format.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
        b->mapped = mmap(NULL, (size_t)gem.size, PROT_WRITE, MAP_SHARED, drmfd, gem_map.offset);    // NOLINT
    } else {
    }

    if (MAP_FAILED == b->mapped) {
        debug_printf("mmap of %s GEM buffer failed %d: %s\n",
               (format.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) ?
               ("CAPTURE") : ("OUTPUT"), errno, strerror(errno));
        return -1;
    }
    b->offset = gem_map.offset;
    b->length = gem.size;

    return 0;
}
#endif

int EncoderProcess::FindDevice(char *dev_path) {
    const char *dev_name = "videox";
    const char *driver_name = "vxe-enc";
    struct v4l2_capability cap = {0};
    char name[512] = "";
    DIR *d;
    struct dirent *dir;
    int fd = -1;

    d = opendir(dev_path);
    if (!d) {
        debug_printf("Failed to open device path %s %d %s\n", dev_path,
               errno, strerror(errno));
        return -1;
    }

    while ((dir = readdir(d)) != NULL) {
        if (strncmp(dir->d_name, dev_name, 5) == 0) {
            strncpy(name, dev_path, sizeof(name)-1);
            strncat(name, dir->d_name, sizeof(name)-1);
            fd = open(name, O_RDWR | O_NONBLOCK, 0);
            if (fd < 0) {
                debug_printf("Failed to open device %s %d %s\n",
                       name, errno, strerror(errno));
                continue;
            }

            memset(&cap, 0, sizeof(cap));

            if (ioctl(fd, VIDIOC_QUERYCAP, &cap)) {
                debug_printf("VIDIOC_QUERYCAP failed on device %s %d %s\n",
                       name, errno, strerror(errno));
                close(fd);
                continue;
            }

            if (strcmp((const char *)cap.driver, driver_name) == 0) {
                debug_printf("No device specified, using %s\n",
                         name);
                break;
            }

            close(fd);
            fd = -1;
        }
    }

    if (fd < 0)
        debug_printf("Failed to find device in %s\n", dev_path);

    return fd;
}

int EncoderProcess::EncoderInit(int vwidth, int vheight) {
    int ret = -1;

    encode_info.n_capbuffers = MAX_CAPBUFS;
    encode_info.n_outbuffers = MAX_OUTBUFS;

    width = vwidth;
    height = vheight;

    debug_printf("--- width=%d height=%d----\n", width, height);

    CalculateFormats(format_name, codec_name, &encode_info.out_format, &encode_info.cap_format,
              width, height);

    encode_info.out_format.memory = out_memory;
    encode_info.cap_format.memory = cap_memory;

    if ((encode_info.out_format.memory == V4L2_MEMORY_DMABUF) ||
        (encode_info.cap_format.memory == V4L2_MEMORY_DMABUF)) {
        if (strlen(drm_name)) {
#ifdef DRM
            /* Known DRM node name */
            drmfd = open(drm_name, O_CLOEXEC | O_RDWR);
            if (drmfd < 0) {
                debug_printf("Failed to open drm device %d %s\n",
                       errno, strerror(errno));
                ret = -1;
                return ret;
            }
#else
            debug_printf("DRM not supported with current build arguments\n");
            ret = -1;
            return ret;
#endif
        } else {
#ifdef DRMMODE
            /* Search DRM directory */
            if (strlen(drm_path) == 0)
                snprintf(drm_path, sizeof(drm_path), "%s",
                     default_drm_path);

            drmfd = find_drm_device(drm_path);
            if (drmfd < 0) {
                debug_printf("Failed to find drm device\n");
                ret = -1;
                return ret;
            }
#else
            debug_printf("Can't search DRM directory\n");
            ret = -1;
            return ret;
#endif
        }
    }

    if (strlen(output_file)) {
        encode_info.wrfd = open(output_file, O_CREAT | O_WRONLY | O_TRUNC, 0777);
        if (0 > encode_info.wrfd) {
            debug_printf("Failed to open output file: %s\n", output_file);
            ret = -1;
        }
        debug_printf("Outputting to %s\n", output_file);
    }

    if (strlen(dev_name)) {
        encode_info.fd = open(dev_name, O_RDWR | O_NONBLOCK, 0);
        if (0 > encode_info.fd) {
            debug_printf("Failed to open device: %s\n", dev_name);
            ret = -1;
            if (0 <= encode_info.wrfd)
                close(encode_info.wrfd);
            }
        debug_printf("Opened device %s\n", dev_name);
    } else {
        /* Search dev directory */
        if (strlen(dev_path) == 0)
            snprintf(dev_path, sizeof(dev_path), "%s",
                 default_dev_path);

        encode_info.fd = FindDevice(dev_path);
        if (encode_info.fd < 0) {
            debug_printf("Failed to find device\n");
            ret = -1;

            if (0 <= encode_info.wrfd)
                close(encode_info.wrfd);
        }
    }

    ret = QueryDevice(encode_info.fd);
    if (ret) {
        close(encode_info.fd);
        return ret;
    }

    ret = CheckFramerate(encode_info.fd, &frameival_num, &frameival_den);
    if (ret) {
        close(encode_info.fd);
        return ret;
    }

    ret = SubscribeEvents(encode_info.fd);
    if (ret) {
        close(encode_info.fd);
        return ret;
    }

    QueryCtrls(encode_info.fd);
    GetCtrls(encode_info.fd);
    TryCtrls(encode_info.fd, bitrate, gop_size, i_period);
    SetCtrls(encode_info.fd, bitrate, gop_size, i_period);
    GetCtrls(encode_info.fd);

    GetParameters(encode_info.fd);
    ret = SetParameters(encode_info.fd, frameival_num, frameival_den);
    if (ret) {
        close(encode_info.fd);
        return ret;
    }

    GetParameters(encode_info.fd);

    GetFormat(encode_info.fd, encode_info.out_format);
    GetFormat(encode_info.fd, encode_info.cap_format);

    TryFormat(encode_info.fd, encode_info.out_format);
    TryFormat(encode_info.fd, encode_info.cap_format);

    ret = SetFormat(encode_info.fd, encode_info.out_format);
    if (ret) {
        UnsubscribeEvents(encode_info.fd);
    }

    ret = SetFormat(encode_info.fd, encode_info.cap_format);
    if (ret) {
        UnsubscribeEvents(encode_info.fd);
    }

    GetFormat(encode_info.fd, encode_info.out_format);
    GetFormat(encode_info.fd, encode_info.cap_format);

    ret = AllocBufs(encode_info.fd, drmfd, &encode_info.n_outbuffers, encode_info.outbuffers,
             V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, encode_info.out_format);
    if (ret) {
        DeallocBufs(encode_info.fd, drmfd, &encode_info.n_outbuffers, encode_info.outbuffers, encode_info.out_format);
        DeallocBufs(encode_info.fd, drmfd, &encode_info.n_capbuffers, encode_info.capbuffers, encode_info.cap_format);
    }

    ret = AllocBufs(encode_info.fd, drmfd, &encode_info.n_capbuffers, encode_info.capbuffers,
             V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, encode_info.cap_format);
    if (ret) {
        DeallocBufs(encode_info.fd, drmfd, &encode_info.n_outbuffers, encode_info.outbuffers, encode_info.out_format);
        DeallocBufs(encode_info.fd, drmfd, &encode_info.n_capbuffers, encode_info.capbuffers, encode_info.cap_format);
    }

    return 0;
}

int EncoderProcess::EncoderYUV(char *yuv, char *stream, int *stream_len) {
    debug_printf("%s: mainloop start \n", __func__);

    if (yuv == NULL) {
        return -1;
    }

    encode_info.yuv_data = yuv;
    encode_info.n_frames = 1;
    encode_info.enc_stream = stream;
    encode_info.stream_len = stream_len;
    StreamingLoop(&encode_info);
    return 0;
}

}  // namespace kapok_hardware_tiencoder

